import math
import random
from time import *
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d.art3d import Poly3DCollection,Line3DCollection
# from Test import box_print
# import pandas as pd


#list减法
def list_subtraction(*args):
    result = list(map(lambda x, y: x - y, args[1], args[0]))
    return result

#list加法
def list_add(x,y):
    result = list(map(lambda x, y: x + y, x, y))
    return result

#输入2个坐标，输出一个向量
def pipe_vector(*args):
    a = list_subtraction(*args)
    return a

def point_2_to_3(p1,p2):
    c = np.add(p1,p2) /2
    b = list(c)
    return [p1,b,p2]

#输入3个点，输出9个点
def point_3_to_9(args):                #坐标3-9
    min_x = args[0][0]
    max_x = args[2][0]
    min_y = args[0][1]
    max_y = args[2][1]
    min_z = args[0][2]
    max_z = args[2][2]

    a1 = [min_x,max_y,max_z]
    a2 = [max_x,max_y,max_z]                  #a前点,b后点,得到的数据类型是list
    a3 = [max_x,max_y,min_z]
    a4 = [min_x,max_y,min_z]
    b1 = [min_x,min_y,max_z]
    b2 = [max_x,min_y,max_z]
    b3 = [max_x,min_y,min_z]
    b4 = [min_x,min_y,min_z]
    c = args[1]
    args = [a1, a2, a3, a4, b1, b2, b3, b4, c]
    return args

#输入3个点，输出8个点
def point_3_to_8(args):                #坐标3-8
    min_x = args[0][0]
    max_x = args[2][0]
    min_y = args[0][1]
    max_y = args[2][1]
    min_z = args[0][2]
    max_z = args[2][2]

    a1 = [min_x,max_y,max_z]
    a2 = [max_x,max_y,max_z]                   #a前点,b后点,得到的数据类型是list
    a3 = [max_x,max_y,min_z]
    a4 = [min_x,max_y,min_z]
    b1 = [min_x,min_y,max_z]
    b2 = [max_x,min_y,max_z]
    b3 = [max_x,min_y,min_z]
    b4 = [min_x,min_y,min_z]

    args = [a1, a2, a3, a4, b1, b2, b3, b4]
    return args

#输入3个点，输出3个面的法向量
def point_3_to_vector_6(args):   #输入3个点坐标，输出六个面，4*6个坐标
    args = point_3_to_9(args)
    surface1_point4 = args[0], args[1], args[2], args[3]  # 面1-6的顺序分别是前后上下左右，每个面的顺序是左上角开始，顺时针走
    surface2_point4 = args[4], args[5], args[6], args[7]
    surface3_point4 = args[4], args[5], args[1], args[0]
    surface4_point4 = args[7], args[6], args[2], args[3]
    surface5_point4 = args[4], args[0], args[3], args[7]
    surface6_point4 = args[5], args[1], args[2], args[6]
    #输出1个大数组,每个数组包括6个元组（代表6个面），每个元组包括4个坐标（代表面的4个点）
    # return [surface1_point4, surface2_point4, surface3_point4, surface4_point4, surface5_point4 ,surface6_point4]
    block_tuple6 = [surface1_point4, surface3_point4, surface5_point4]

    #元组转成数组，便于计算
    u = []
    for j in block_tuple6:
        # print(j)
        for i in j:
            u.append(i)
    print(u)

    j = 0
    while(j<24):
        # #计算每个面的法向量
        surface1_vector_a = list(map(lambda x, y: x - y, u[j], u[j+1]))  # map映射对应数值关系；lambda表达式
        # print(surface1_vector_a)
        surface1_vector_b = list(map(lambda x, y: x - y, u[j], u[j+3]))
        # print(surface1_vector_b)
        normal_vector_i = surface1_vector_a[1] * surface1_vector_b[2] - surface1_vector_a[2] * surface1_vector_b[1]
        normal_vector_j = surface1_vector_a[2] * surface1_vector_b[0] - surface1_vector_a[0] * surface1_vector_b[2]
        normal_vector_k = surface1_vector_a[0] * surface1_vector_b[1] - surface1_vector_a[1] * surface1_vector_b[0]
        print([normal_vector_i, normal_vector_j, normal_vector_k])
        j += 4  #

#输入面的4个坐标，输出1个法向量
def find_normal_vector(*args):
    surface_vector1 = list_subtraction(args[0], args[1])        #map映射对应数值关系；lambda表达式
    surface_vector2 = list_subtraction(args[0], args[3])
    normal_vector_i = surface_vector1[1] * surface_vector2[2] - surface_vector1[2] * surface_vector2[1]
    normal_vector_j = surface_vector1[2] * surface_vector2[0] - surface_vector1[0] * surface_vector2[2]
    normal_vector_k = surface_vector1[0] * surface_vector2[1] - surface_vector1[1] * surface_vector2[0]
    return [normal_vector_i, normal_vector_j, normal_vector_k]

#输入3个点，输出6个面，用面的4点坐标表示
def point_3_to_surface_6(args):  # 输入3个点坐标，输出六个面，4*6个坐标
    args = point_3_to_9(args)
    surface1_point4 = [args[0], args[1], args[2], args[3]] # 面1-6的顺序分别是前后上下左右，每个面的顺序是左上角开始，顺时针走
    surface2_point4 = [args[4], args[5], args[6], args[7]]
    surface3_point4 = [args[4], args[5], args[1], args[0]]
    surface4_point4 = [args[7], args[6], args[2], args[3]]
    surface5_point4 = [args[4], args[0], args[3], args[7]]
    surface6_point4 = [args[5], args[1], args[2], args[6]]
    # 输出1个大数组,每个数组包括6个元组（代表6个面），每个元组包括4个坐标（代表面的4个点）
    # return [surface1_point4, surface2_point4, surface3_point4, surface4_point4, surface5_point4 ,surface6_point4]
    block_tuple6 = [surface1_point4, surface2_point4, surface3_point4, surface4_point4, surface5_point4,
                    surface6_point4]
    return block_tuple6

#简易判断(仅用于框选障碍物时，一直垂直于XYZ的情况)
#输入3个点，输出3个面的法向量
def quick_point_3_to_vector(*args):
    args = point_3_to_9(*args)
    vector_3 = []
    surface_x_vector = list(map(lambda x, y: x - y, args[1], args[0]))  # map映射对应数值关系；lambda表达式
    surface_y_vector = list(map(lambda x, y: x - y, args[0], args[4]))
    surface_z_vector = list(map(lambda x, y: x - y, args[0], args[3]))
    vector_3.append(surface_x_vector)
    vector_3.append(surface_y_vector)
    vector_3.append(surface_z_vector)
    return vector_3

#输入2个向量，输出角度和线面关系，return线面度数
def vector_2_angle_calculation(vec1,vec2):
    angle_set1 = 30
    angle_set2 = 180 - angle_set1
    a_list = []
    ji = 0
    mo_vec1 = 0
    mo_vec2 = 0
    for i in range(3):
        a = vec1[i]*vec2[i]
        a_list.append(a)
        ji += a
        c = math.sqrt(vec1[i] * vec1[i])
        mo_vec1 += c
        d = math.sqrt(vec2[i] * vec2[i])
        mo_vec2 += d
    # print(ji)
    # print(mo_vec1,mo_vec2)
    sin_vec = 100*round(ji/mo_vec1/mo_vec2,2)
    print("线面的角度为：" + str(90 - sin_vec) + "度")

    #判断线面关系
    if( 0<sin_vec<angle_set1 or angle_set2<sin_vec<180 ):
        print("线面平行")
    elif(sin_vec == 0 or sin_vec == 180):
        print("线面垂直")
    else:
        print("线面交叉")
    return 90 - sin_vec

#输入面的4个坐标，输出面的解析式
def define_surface(args=[]):
    # print(*args)
    #点法式方程：A(x - x0) + B(y - y0) + C(z - z0) = Ax + By + Cz - (Ax0 + By0 + Cz0)
    #return:（Ax, By, Cz, D）代表：Ax + By + Cz + D = 0
    point1 = np.asarray(args[0])
    point2 = np.asarray(args[1])
    point3 = np.asarray(args[3])
    AB = np.asmatrix(point2 - point1)
    AC = np.asmatrix(point3 - point1)
    a = np.cross(AB, AC)  # 向量叉乘，求法向量
    N1 = np.linalg.norm(a)
    N = np.divide(a,N1)
    Ax = N[0, 0]
    By = N[0, 1]
    Cz = N[0, 2]
    D = -(Ax * point1[0] + By * point1[1] + Cz * point1[2])
    return [Ax, By, Cz, D]

#计算点面距离
#输入1个点，一个面的4个点，return距离
def point_to_surface_distance(point, args=[]):
    Ax, By, Cz, D = define_surface([args[0], args[1], args[2], args[3]])
    mod_d = Ax * point[0] + By * point[1] + Cz * point[2] + D
    mod_area = np.sum(np.square([Ax, By, Cz]))
    d = mod_d / np.sqrt(mod_area)
    return d

#计算点面距离。默认step = 10，距离小于0.1则为0。
#输入管道2个点，面的4个点，return碰撞信息；
def find_crash(point1, point2, *args):
    # print(point1)
    # print(point2)
    a = list_subtraction(point1, point2)
    # print(a)
    #计算xyz的步长
    step = 10
    x_step = abs(a[0] / step)
    y_step = abs(a[1] / step)
    z_step = abs(a[2] / step)
    # print(type(point1))
    # print(type(step_point))
    #可优化部分
    all_point = [point1, point2]
    #设置小于某值的时候，当作等于0
    min_d = 0.1
    #设置碰撞提示，0时为无碰撞，1时为发生碰撞
    crash_flag = 0
    #计算点1是否为交点
    point1_d = point_to_surface_distance(point1, *args)
    # print(point1_d)
    #计算其余点是否为交点
    if(point1_d > min_d):
        for i in range(0, step+1):
            step_point = [x_step * i, y_step * i, z_step * i]
            new = list_subtraction(step_point, point2)
            all_point.append(new)
            new_d = point_to_surface_distance(new, *args)
            # print(new_d)
            if(new_d > min_d ):
                # print('continue')
                continue
            else:
                print("碰撞点坐标为：")
                print(new)
                crash_flag = 1
                return  new
    else:
        print("碰撞点坐标为：")
        print(point1)
        crash_flag = 1
        return point1
    if(crash_flag == 1):
        print("发生碰撞")
    else:
        print("没有发生碰撞")
        return None

# 输入管道2个点，面的4个点，可设置参数，return碰撞信息
def find_crash_set_parameter(step_set, min_distance, point1, point2, *args):
    # print(point1)
    # print(point2)
    a = list_subtraction(point1, point2)
    # print(a)
    #计算xyz的步长
    step = step_set
    x_step = abs(a[0] / step)
    y_step = abs(a[1] / step)
    z_step = abs(a[2] / step)
    # print(type(point1))
    # print(type(step_point))
    #可优化部分
    all_point = [point1, point2]
    #设置小于某值的时候，当作等于0
    min_d = min_distance
    #设置碰撞提示，0时为无碰撞，1时为发生碰撞
    crash_flag = 0
    #计算点1是否为交点
    point1_d = point_to_surface_distance(point1, *args)
    # print(point1_d)
    #计算其余点是否为交点
    if(point1_d > min_d):
        for i in range(0, step+1):
            step_point = [x_step * i, y_step * i, z_step * i]
            new = list_subtraction(step_point, point2)
            all_point.append(new)
            new_d = point_to_surface_distance(new, *args)
            # print(new_d)
            if(new_d > min_d ):
                # print('continue')
                continue
            else:
                print("碰撞点坐标为：")
                print(new)
                crash_flag = 1
                return  new
    else:
        print("碰撞点坐标为：")
        print(point1)
        crash_flag = 1
        return point1
    if(crash_flag == 1):
        print("发生碰撞")
    else:
        print("没有发生碰撞")
        return None

#输入管道2个点，面的4个点，输出碰撞类型
def crash_type(point1, point2, *args):
    a = pipe_vector(point1, point2)
    # print(*args)
    b = find_normal_vector(*args)
    # print(b)
    vector_2_angle_calculation(a, b)

#赋予高度信息
def height_given(*args):
    pass

#坐标差值是否当0处理，微调
def adjust_judge(step, point1, point2):
    if (abs(point1 - point2) < step):
        return 1
    else:
        return 0

#输入管道坐标，输出管道绑定
def pipes_link(*args):
    pass

#输入所有管道坐标，进行分类
def pipes_to_area(*args):
    pass

#输入所有障碍物坐标，进行区域划分
def block_to_area(*args):
    pass

#引入中间点2
#输入管道2个坐标，障碍物3个坐标，输出管道新生成的2个坐标

#输入障碍物三个坐标，输出障碍物类型
def block_type(args):
    a = point_3_to_9(args)
    # print(a)
    length = abs(list_subtraction(a[1], a[0])[0])
    width = abs(list_subtraction(a[4], a[0])[1])
    height = abs(list_subtraction(a[0], a[3])[2])
    limit = 4
    times = 2
    lw_rate = length/width
    lh_rate = length/height
    wh_rate = width/height
    if(lw_rate > limit):
        # print("beam")
        return "beam"
    elif(lw_rate < 1/limit):
        # print("beam")
        return "beam"
    elif(wh_rate < 1/limit and lh_rate < 1/limit):
        # print("column")
        return "column"
    else:
        # print("wall")
        return "wall"

#完成避障生成中间点
#输入管道2个坐标，障碍物三个坐标，输出管道新生成的2个坐标
def crash_to_point_create(point1, point2, *args):
    pass

#输入3个坐标，输出6个面的解析式
def define_all_surface(*args):
    #点法式方程：A(x - x0) + B(y - y0) + C(z - z0) = Ax + By + Cz - (Ax0 + By0 + Cz0)
    #return:（Ax, By, Cz, D）代表：Ax + By + Cz + D = 0
    a = []
    args = point_3_to_9(*args)
    surface1 = [args[0], args[1], args[2], args[3]]  # 面1-6的顺序分别是前后上下左右，每个面的顺序是左上角开始，顺时针走
    surface2 = [args[4], args[5], args[6], args[7]]
    surface3 = [args[4], args[5], args[1], args[0]]
    surface4 = [args[7], args[6], args[2], args[3]]
    surface5 = [args[4], args[0], args[3], args[7]]
    surface6 = [args[5], args[1], args[2], args[6]]
    for i in range(1,7):
        a.append(eval("surface" + str(i)))
    surface_6 =[]
    for j in range(6):
        # print(j)
        args = a[j]
        # print(args)
        point1 = np.asarray(args[0])
        point2 = np.asarray(args[1])
        point3 = np.asarray(args[3])
        AB = np.asmatrix(point2 - point1)
        AC = np.asmatrix(point3 - point1)
        N = np.cross(AB, AC)  # 向量叉乘，求法向量
        # Ax+By+Cz
        Ax = N[0, 0]
        By = N[0, 1]
        Cz = N[0, 2]
        D = -(Ax * point1[0] + By * point1[1] + Cz * point1[2])
        surface_6.append([Ax, By, Cz, D])
    # print(surface_6)
    return surface_6

#查找管段附近障碍物
#输入所有管道2个坐标，所有障碍物6个面的坐标，返回范围内障碍物最近距离
def Search_block(*args1,**args2):
    pass

#____________________________________________________________________
#生成邻近点，输入点，return点
def create_near(args):
    point_x = args[0]
    # print(point_x)
    point_y = args[1]
    map_size = 1000
    if (random.randint(0, 100) > 50):
        point_y = random.randint(0, map_size)
        re = [point_x,point_y]
        # print(re)
        return re
    else:
        point_x = random.randint(0, map_size)
        re = [point_x, point_y]
        # print(re)
        return re

#生成小段管，随机输出两个数组代表一节管
def pi_bind(map_size):
    pi_list = []
    map_size = map_size
    point_x = random.randint(0, map_size)
    point_y = random.randint(0, map_size)
    start = [point_x,point_y]
    if (random.randint(0, 100) > 50):
        pi_list.append(start)
        point_y = random.randint(0, map_size)
        pi_list.append([point_x,point_y])
        # print(pi_list)
    else:
        pi_list.append(start)
        point_x = random.randint(0, map_size)
        pi_list.append([point_x, point_y])
        # print(pi_list)
    return pi_list

#管段接续，一节变N节,输入M节，return一条管道
def pi_1_to_N(M):
    # section代表生成5节管道
    section = M
    p1 = pi_bind(1000)
    n_list = [p1]
    for i in range(section-1):
        temp_list = [[],[]]
        # print(p1[1])
        temp_list[0] = p1[1]
        # print(temp_list)
        # print(temp_list[0])
        temp_list[1] = create_near(temp_list[0])
        n_list.append(temp_list)
        p1 = temp_list
    return n_list
    # print(n_list)

#生成3条管，每条5节管道，一节管道用一个起点和一个终点表示
def create_N_M(*args):
    num = args[0]
    section = args[1]
    p_num = []
    for i in range(num):
        # p_num = []
        p1 = pi_1_to_N(section)
        p_num.append(p1)
        #两种输出管道的方式
    print(p_num)
    a = np.array(p_num)
    print(a)
    return a

#______________________________________________________________________
#生成邻近点，输入点，return点
def create_near_3D(args):
    point_x = args[0]
    point_y = args[1]
    point_z = args[2]
    map_size = 1000
    flag = random.randint(0,3)#函数
    if (flag == 0):
        point_x = random.randint(0, map_size)
        re = [point_x,point_y,point_z]
        # print(re)
        return re
    elif(flag == 1):
        point_y = random.randint(0, map_size)
        re = [point_x, point_y, point_z]
        # print(re)
        return re
    else:
        point_z = args[2] - args[2]/random.randint(1, 5)
        re = [point_x, point_y, point_z]
        # print(re)
        return re

#生成小段管，随机输出两个数组代表一节管
def pi_bind_3D(map_size):
    pi_list = []
    map_size = map_size
    level = 200
    point_x = random.randint(0, map_size)
    point_y = random.randint(0, map_size)
    point_z = random.randint(map_size - level, map_size)
    start = [point_x,point_y,point_z]
    if (random.randint(0, 100) > 33):
        pi_list.append(start)
        point_x = random.randint(0, map_size)
        pi_list.append([point_x,point_y,point_z])
        # print(pi_list)
    elif(66> random.randint(0, 100) > 33):
        pi_list.append(start)
        point_y = random.randint(0, map_size)
        pi_list.append([point_x, point_y,point_z])
        # print(pi_list)
    else:
        pi_list.append(start)
        point_z = random.randint(0, map_size)
        pi_list.append([point_x, point_y, point_z])
        # print(pi_list)
    return pi_list

#管段接续，一节变N节,输入M节，return一条管道 (用于处理“每节管道”)
def pi_1_to_N_3D(M):
    # section代表生成5节管道
    section = M
    p1 = pi_bind_3D(1000)
    n_list = [p1]
    for i in range(section-1):
        temp_list = [[],[]]
        # print(p1[1])
        temp_list[0] = p1[1]
        temp_list[1] = create_near_3D(temp_list[0])
        n_list.append(temp_list)
        p1 = temp_list
        # print(n_list)
    return n_list
    #

#生成3条管，每条5节管道，一节管道用一个起点和一个终点表示
def create_N_M_3D(*args):
    num = args[0]
    section = args[1]
    p_num = []
    for i in range(num):
        # p_num = []
        p1 = pi_1_to_N_3D(section)
        p_num.append(p1)
        #两种输出管道的方式
    # print(p_num)
    a = np.array(p_num)
    # print(a)
    return p_num

#______________________________________________________________________
#绘一个障碍物图，输入八个坐标点,输出图
def drawing_one_block(*args):
    fig = plt.figure()
    ax = fig.gca(projection='3d')
    verts = [args[0][0], args[0][1], args[0][2], args[0][3], args[0][4], args[0][5], args[0][6], args[0][7]]
    # 每个面的点的索引号（固定）
    faces = [[0, 1, 2, 3], [4, 5, 6, 7], [0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [0, 3, 7, 4]]
    # 获得每个面的顶点
    poly3d = [[verts[vert_id] for vert_id in face] for face in faces]

    # 绘制顶点
    x, y, z = zip(*verts)
    ax.scatter(x, y, z)
    # 绘制多边形面
    ax.add_collection3d(Poly3DCollection(poly3d, facecolors='blue', linewidths=1, alpha=0.3))
    # 绘制对变形的边
    ax.add_collection3d(Line3DCollection(poly3d, colors='red', linewidths=0.5, linestyles=':'))

    # 设置图形坐标范围
    ax.set_xlabel('X')
    ax.set_xlim3d(0, 1000)
    ax.set_ylabel('Y')
    ax.set_ylim3d(0, 1000)
    ax.set_zlabel('Z')
    ax.set_zlim3d(0, 1000)
    plt.show()

#随机生成障碍物坐标,三点坐标(元祖)
def block_random_3_point(*args):
    left = 800
    more = 400
    a = np.random.randint(0, left, 3)
    b = a + np.random.randint(0, more/4, 3)
    c = (a + b) / 2
    al = np.ndarray.tolist(a)
    bl = np.ndarray.tolist(b)
    cl = np.ndarray.tolist(c)
    abc = [al,cl,bl]
    q = [tuple(x) for x in abc]
    tu = tuple(q)
    # print(tu)
    return tu

#随机生成N个障碍物绘图坐标元祖
def block_random_all(block_num):
    num = block_num
    list_all = []
    for i in range(num):
        #八个坐标
        b = point_3_to_8(block_random_3_point())
        list_all.append(b)
    # print(list_all)
    return list_all

#绘N障碍物图，输入N*m个坐标点,输出所有图（待修改障碍物不重叠）
def drawing_add_blocks(args):
    print(args)
    fig = plt.figure()
    ax = fig.gca(projection='3d')
    # ploy3d = []
    faces = [[0, 1, 2, 3], [4, 5, 6, 7], [0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [0, 3, 7, 4]]
    verts = args
    for i in range(len(verts)):
        # print(i)
        # print(verts[i])
    # # 获得6个面的4个顶点
        all_face_4_point = [[verts[i][vert_id] for vert_id in face] for face in faces]
        # print(all_face_4_point)
    # 绘制顶点
        x,y,z = zip(*verts[i])
        # print(x,y,z)
        ax.scatter(x, y, z)
        x,y,z = 0,0,0
        # print(x,y,z)

        # 绘制多边形面
        ax.add_collection3d(Poly3DCollection(all_face_4_point, facecolors='blue', linewidths=1, alpha=0.3))
        # 绘制对变形的边
        ax.add_collection3d(Line3DCollection(all_face_4_point, colors='red', linewidths=0.5, linestyles=':'))
        # 设置图形坐标范围
        ax.set_xlabel('X')
        ax.set_xlim3d(0, 1000)
        ax.set_ylabel('Y')
        ax.set_ylim3d(0, 1000)
        ax.set_zlabel('Z')
        ax.set_zlim3d(0, 1000)
    # plt.show()

#随机生成管道坐标,三点坐标(元祖)
def pipe_random_3_point(*args):
    pass

#输入节管道两个点，生成八个点（线到立体的扩充，用于绘图）
def point_2_to_8(args):
    DN = 10
    R = DN/2
    judge = np.array(args[1])-np.array(args[0])
    c = (np.array(args[1]) + np.array(args[0])) / 2
    if(judge[0] != 0):
        flag = 0
        p1 = [args[0][0], args[0][1] - R, args[0][2]]
        p2 = [args[1][0], args[1][1] + R, args[1][2]]
        a = point_3_to_8([p1, c, p2])
        return a

    elif(judge[1] != 0):
        flag = 1
        p1 = [args[0][0] - R, args[0][1], args[0][2]]
        p2 = [args[1][0] + R, args[1][1], args[1][2]]
        a = point_3_to_8([p1, c, p2])
        return a

    else:
        flag = 2
        p1 = [args[0][0] - R, args[0][1] - R, args[0][2]]
        p2 = [args[1][0] + R, args[1][1] + R, args[1][2]]
        a = point_3_to_8([p1, c, p2])
        return a

#输入节N*M管道两个点，生成管道绘图数据（线到立体的扩充，用于绘图）
def all_point_2_to_8(args):
    all_2 = []
    a = np.array(args)
    # len（a）数值表示几条管道
    N = len(a)
    b = a.reshape(int(a.size/6),2,3)
    each_2 = np.ndarray.tolist(b)
    M = int(len(each_2)/len(a))
    # print(len(each_2))
    for i in each_2:
        all_2.append(point_2_to_8(i))
    # new_all_2 = np.array(all_2).reshape(len(a),int(a.size/6/len(a)),2,3)
    new_all_2 = np.array(all_2).reshape(N,M, 8, 3)
    new_all_2 = np.ndarray.tolist(new_all_2)
    # print(new_all_2)
    # print(all_2)
    return new_all_2



    # q = [tuple(x) for x in each_2]
    # tu = tuple(q)
    # print(tu)
    # print(tu[0])
    # for i in tu:

#绘N*M管道图，输入N*m个坐标点,输出所有图（待完善）
def drawing_add_pipes(args):
    fig = plt.figure()
    ax = fig.gca(projection='3d')
    faces = [[0, 1, 2, 3], [4, 5, 6, 7], [0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [0, 3, 7, 4]]
    verts = args
    for i in range(len(verts)):
        # # 获得6个面的4个顶点
        all_face_4_point = [[verts[i][vert_id] for vert_id in face] for face in faces]
        # print(all_face_4_point)
        # 绘制顶点
        x, y, z = zip(*verts[i])
        # print(x, y, z)
        ax.scatter(x, y, z)
        x, y, z = 0, 0, 0
        # print(x,y,z)
        # 绘制多边形面
        ax.add_collection3d(Poly3DCollection(all_face_4_point, facecolors='black', linewidths=1, alpha=0.5))
        # 绘制对变形的边
        ax.add_collection3d(Line3DCollection(all_face_4_point, colors='red', linewidths=0.5, linestyles=':'))
        # 设置图形坐标范围
        ax.set_xlabel('X')
        ax.set_xlim3d(0, 1000)
        ax.set_ylabel('Y')
        ax.set_ylim3d(0, 1000)
        ax.set_zlabel('Z')
        ax.set_zlim3d(0, 1000)
        # plt.show
    # plt.show()

def drawing_add_all(pipes_N,pipes_M,block_num):
    pipes = all_point_2_to_8(create_N_M_3D(pipes_N, pipes_M))
    blocks = block_random_all(block_num)
    fig = plt.figure()
    ax = fig.gca(projection='3d')
    faces = [[0, 1, 2, 3], [4, 5, 6, 7], [0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [0, 3, 7, 4]]
    verts1 = pipes
    verts2 = blocks

    for i in range(len(verts1)):
        all_face_4_point1 = [[verts1[i][vert_id] for vert_id in face] for face in faces]
        x, y, z = zip(*verts1[i])
        ax.scatter(x, y, z)

        ax.add_collection3d(Poly3DCollection(all_face_4_point1, facecolors='red', linewidths=1, alpha=0.3))
        ax.add_collection3d(Line3DCollection(all_face_4_point1, colors='red', linewidths=0.5, linestyles=':'))


    for j in range(len(verts2)):
        all_face_4_point2 = [[verts2[j][vert_id] for vert_id in face] for face in faces]
        x, y, z = zip(*verts2[j])
        ax.scatter(x, y, z)

        ax.add_collection3d(Poly3DCollection(all_face_4_point2, facecolors='black', linewidths=1, alpha=0.5))
        # 绘制对变形的边
        ax.add_collection3d(Line3DCollection(all_face_4_point2, colors='blue', linewidths=0.5, linestyles=':'))
        # 设置图形坐标范围
    ax.set_xlabel('X')
    ax.set_xlim3d(0, 1000)
    ax.set_ylabel('Y')
    ax.set_ylim3d(0, 1000)
    ax.set_zlabel('Z')
    ax.set_zlim3d(0, 1000)

def drawing_add_all_design(pipes_N,pipes_M,block_num):
    pipes = all_point_2_to_8(create_N_M_3D(pipes_N, pipes_M))
    blocks = block_random_all(block_num)

    a = np.array(pipes)
    b = a.reshape(pipes_N, int(a.size / 3 / pipes_N), 3)

    all = []
    step = 50
    for j in range(pipes_N):
        x_list = []
        y_list = []
        z_list = []
        for i in b[j]:

            x_list.append(i[0])
            y_list.append(i[1])
            z_list.append(i[2])
        x = max(x_list) + step
        y = max(y_list) + step
        z = max(z_list) + step
        x1 = min(x_list) - step
        y1 = min(y_list) - step
        z1 = min(z_list) - step
        box_2 = [x, y, z]
        box_1 = [x1, y1, z1]
        box_c = [0, 0, 0]
        space = point_3_to_8([box_1,box_c,box_2])
        all.append(space)
    print(all)


    fig = plt.figure()
    ax = fig.gca(projection='3d')
    faces = [[0, 1, 2, 3], [4, 5, 6, 7], [0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [0, 3, 7, 4]]
    verts1 = pipes
    verts2 = blocks
    verts3 = all



    # 绘制管道图
    for i in range(len(verts1)):
        all_face_4_point1 = [[verts1[i][vert_id] for vert_id in face] for face in faces]
        x, y, z = zip(*verts1[i])
        ax.scatter(x, y, z)

        ax.add_collection3d(Poly3DCollection(all_face_4_point1, facecolors='red', linewidths=1, alpha=0.3))
        ax.add_collection3d(Line3DCollection(all_face_4_point1, colors='red', linewidths=0.5, linestyles=':'))

    # 绘制空间划分图
    for i in range(len(verts3)):
            all_face_4_point_box = [[verts3[i][vert_id] for vert_id in face] for face in faces]
            xb, yb, zb = zip(*verts3[i])
            ax.scatter(xb, yb, zb)

            ax.add_collection3d(Poly3DCollection(all_face_4_point_box, facecolors='pink', linewidths=0.5, alpha=0.3))
            ax.add_collection3d(Line3DCollection(all_face_4_point_box, colors='pink', linewidths=2, linestyles=':'))

    #绘制障碍物图
    for j in range(len(verts2)):
        all_face_4_point2 = [[verts2[j][vert_id] for vert_id in face] for face in faces]
        x, y, z = zip(*verts2[j])
        ax.scatter(x, y, z)

        ax.add_collection3d(Poly3DCollection(all_face_4_point2, facecolors='black', linewidths=1, alpha=0.5))
        # 绘制对变形的边
        ax.add_collection3d(Line3DCollection(all_face_4_point2, colors='blue', linewidths=0.5, linestyles=':'))
        # 设置图形坐标范围
    ax.set_xlabel('X')
    ax.set_xlim3d(0, 1000)
    ax.set_ylabel('Y')
    ax.set_ylim3d(0, 1000)
    ax.set_zlabel('Z')
    ax.set_zlim3d(0, 1000)
    return verts1
#输入管道和障碍物的坐标，对障碍物进行绑定和区域的划分
def pipe_to_region(pipes,blocks):
    import numpy as np
    from numpy.core.fromnumeric import shape
    import copy
    a = []
    b = []
    c = []
    e1 = []
    e2 = []
    e3 = []
    area = []
    area1 = []
    area2 = []
    area3 = []
    block = []
    block1 = []
    block2 = []
    temp = 100

    for w in range(len(pipes)):
        a = pipes[w]
        b=np.array(a)
        d=np.array(b.reshape(int(b.size/3),3))
        e = d.tolist()
        for i in range(len(e)):
            e1.append(e[i][0])
            e2.append(e[i][1])
            e3.append(e[i][2])
        g1 = [min(e1),min(e2),min(e3)]
        g2 = [max(e1),max(e2),max(e3)]
        c.append([g1,g2])
        if c[0][0][0] > temp and c[0][0][1] > temp and c[0][0][2 ]> temp:
            area.append([[c[0][0][0]-temp,c[0][0][1]-temp,c[0][0][2]-temp],[c[0][1][0]+temp,c[0][1][1]+temp,c[0][1][2]+temp]])
        else:
             area.append([c[0][0],[c[0][1][0] + temp, c[0][1][1] + temp, c[0][1][2] + temp]])
        area3 = [area[0][0],[(area[0][0][0]+area[0][1][0])/2,(area[0][0][1]+area[0][1][1])/2,(area[0][0][2]+area[0][1][2])/2],area[0][1]]
        area4 = point_3_to_8(area3)
        area2.append(area4)
        c.clear()
        e1.clear()
        e2.clear()
        e3.clear()
        for v in range(len(blocks)):
            f = copy.deepcopy(blocks[v])
            f.append([(f[0][0]+f[1][0])/2,(f[0][1]+f[5][1])/2,(f[0][2]+f[2][2])/2])
            i = 0
            while i<=8:
                if area[0][0][0] <= f[i][0] <= area[0][1][0] and area[0][0][1] <= f[i][1] <= area[0][1][1] and area[0][0][2] <= f[i][2] <= area[0][1][2]:
                    if len(area1) == 0:
                        area1.append(blocks[v])
                        print(area1)

                i+=1
            block.extend(area1)
            area1.clear()
            f.clear()
            block1 = copy.deepcopy(block)
        block.clear()
        # print("第",w+1,"条管道的区域为",area,"绑定的障碍物为",area1)
        block2.append(block1)
        area.clear()
    print("所有管道区域分别绑定的障碍物为",block2)
    print("所有管道的区域为",area2)
    return area2,block2


